#include "vehicle.h"
#include <ctime>
#include <string>
#include <sstream>
#include <sys/time.h>

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

static void halt(int eno){
    exit(eno);
}

static std::shared_ptr<mavsdk::System> get_system(Mavsdk& mavsdk){
    std::cout << "Waiting to discover system...\n";
    auto prom = std::promise<std::shared_ptr<System>>{};
    auto fut = prom.get_future();

    // We wait for new systems to be discovered, once we find one that has an
    // autopilot, we decide to use it.
    Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
        auto system = mavsdk.systems().back();

        if (system->has_autopilot()) {
            std::cout << "Discovered autopilot\n";

            // Unsubscribe again as we only want to find one system.
            mavsdk.unsubscribe_on_new_system(handle);
            prom.set_value(system);
        }
    });

    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds max, surely.
    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
        std::cerr << "No autopilot found.\n";
        halt(1);
    }

    // Get discovered system now.
    return fut.get();
}

mavsdk::ConnectionResult Vechile::connect(char *connection_url){
    _connection_result = _mavsdk.add_any_connection(connection_url);
    if (_connection_result != ConnectionResult::Success) {
        std::cerr << "Connection failed: " << _connection_result << std::endl;
        halt(1);
    }
    return _connection_result;
}

Vechile::Vechile(char *connection_url)
    :_mavsdk(),
    _connection_result(connect(connection_url)), 
    _system(get_system(_mavsdk)), 
    _telemetry(Telemetry{_system}), 
    _action(Action{_system}){

    // We want to listen to the altitude of the drone at 1 Hz.
    const auto set_rate_result = _telemetry.set_rate_position(1.0);
    if (set_rate_result != Telemetry::Result::Success) {
        std::cerr << "Setting rate failed: " << set_rate_result << std::endl;
        halt(3);
    }

    // Set up callback to monitor altitude while the vehicle is in flight
    _telemetry.subscribe_position([this](Telemetry::Position position) {
        _tl.Latitude = position.latitude_deg;
        _tl.Longitude = position.longitude_deg;
        _tl.PlatformHeight = position.relative_altitude_m;
    });

    _telemetry.subscribe_attitude_euler([this](Telemetry::EulerAngle eulerAngle){
        _tl.TimeStamp = eulerAngle.timestamp_us;
        _tl.Pitch = eulerAngle.pitch_deg;
        _tl.Yaw = eulerAngle.yaw_deg;
        _tl.Roll = eulerAngle.roll_deg;
    });
}

int Vechile::arm(){
    // Check until vehicle is ready to arm
    //while (_telemetry.health_all_ok() != true) {
    //    std::cout << "Vehicle is getting ready to arm\n";
    //    sleep_for(seconds(1));
    //}

    // Arm vehicle
    std::cout << "Arming...\n";
    const Action::Result arm_result = _action.arm();

    if (arm_result != Action::Result::Success) {
        std::cerr << "Arming failed: " << arm_result << '\n';
        return -1;
    }

    return 0;
}
int Vechile::disarm(){
	_action.disarm();
}
int Vechile::takeoff(){
    // Take off
    std::cout << "Taking off...\n";
    const Action::Result takeoff_result = _action.takeoff();
    if (takeoff_result != Action::Result::Success) {
        std::cerr << "Takeoff failed: " << takeoff_result << '\n';
        return -1;
    }

    // Let it hover for a bit before landing again.
    sleep_for(seconds(1));
    return 0;
}

int Vechile::land(){
    std::cout << "Landing...\n";
    const Action::Result land_result = _action.land();
    if (land_result != Action::Result::Success) {
        std::cerr << "Land failed: " << land_result << '\n';
        return -1;
    }

    // Check if vehicle is still in air
    while (_telemetry.in_air()) {
        std::cout << "Vehicle is landing...\n";
        sleep_for(seconds(1));
    }
    std::cout << "Landed!\n";
    return 0;
}

TelemetryLite::TelemetryLite()
    :UniqueID(0),TimeStamp(0), Longitude(0), Latitude(0),
    PlatformHeight(0), Yaw(0), Pitch(0), Roll(0),
    VFov(0), HFov(0), FocalLength35mmEq(0){
}

void TelemetryLite::dump(){
    std::cout << toJson() << std::endl;
}

std::string TelemetryLite::toJson(){
    std::stringstream ss;
    ss << "{";
    ss << "\"geometry\": {";
    ss << "\"coordinates\": [";
    ss << "9.8072370660407,";
    ss << "172.80638454222";
    ss << "],";
    ss << "\"type\": \"Point\"";
    ss << "},";
    ss << "\"type\": \"Feature\",";
    ss << "\"properties\": {";
    ss << "\"timestamp\":" << TimeStamp << ",";
    ss << "\"x\":" << Longitude << ",";
    ss << "\"y\":" << Latitude << ",";
    ss << "\"z\":" << PlatformHeight << ",";
    ss << "\"yaw\":" << Yaw << ",";
    ss << "\"pitch\":" << Pitch << ",";
    ss << "\"roll\":" << Roll << ",";
    ss << "\"fovX\":" << VFov << ",";
    ss << "\"fovY\":" << HFov << ",";
    ss << "\"CameraID\": 1";
    ss << "}";
    ss << "}";
    return ss.str();
}

TelemetryLite& Vechile::readTelemetry(){
#if 0
    std::stringstream strtime;
    std::time_t currenttime = std::time(0);	
    char tAll[255];
    std::strftime(tAll, sizeof(tAll), "%Y/%m/%d %H:%M", std::localtime(&currenttime));
    strtime << tAll;
    _tl.DateTime = strtime.str();
    _tl.UniqueID++;
#endif
    struct timeval tv;
    gettimeofday(&tv, 0);
    _tl.TimeStamp = tv.tv_sec * 1000 + tv.tv_usec / 1000;
    return _tl;
}
